package algorithms;
import util.SensorColour;
import bluetooth.RemoteControl;
import lejos.robotics.navigation.DifferentialPilot;


public class LineFinder {
	
	private RemoteControl rc;
	private final static float SECTOR_SIZE = 40;
	private final static float X_SONAR_OFFSET = 4;
	private final static float Y_SONAR_OFFSET = 2;
	
	public LineFinder(RemoteControl rc)
	{
		this.rc = rc;
	}
	
	public void run(SensorColour colour) throws InterruptedException
	{	if(!findLine())
			return;
		
		DifferentialPilot pilot = rc.getPilot();
		double travelSpeed = pilot.getTravelSpeed();
		double rotateSpeed = pilot.getRotateSpeed();
		pilot.setTravelSpeed(10);
		pilot.setRotateSpeed(40);

		pilot.forward();
		while(rc.getLightColour() != colour) 
		{
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		rc.immediateStop();
		pilot.travel(RemoteControl.SENSOR_TO_WHEEL_DISTANCE);
		pilot.rotateLeft();
		
		while(rc.getLightColour() == colour) 
		{
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		
		while(rc.getLightColour() != colour) 
		{
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		rc.immediateStop();
		
		pilot.setRotateSpeed(5);
		pilot.rotateRight();
		while(rc.getLightColour() == colour) 
		{
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		rc.immediateStop();
		
		pilot.setRotateSpeed(rotateSpeed);
		pilot.setTravelSpeed(travelSpeed);
		pilot.rotate(-74.5);
		
		pilot.travel(-SECTOR_SIZE/2);
	}
	public synchronized void goToMiddle() throws InterruptedException{
		DifferentialPilot pilot =rc.getPilot();
		float xSonarDistance;
		float ySonarDistance;
		rc.rotateSonarMotorTo(0);
		Thread.sleep(500);
		xSonarDistance = rc.getSonarDistance();
		rc.rotateSonarMotorTo(90);
		Thread.sleep(500);
		ySonarDistance = rc.getSonarDistance();
		if(xSonarDistance>40){
			if(ySonarDistance>40){
				rc.rotateSonarMotorTo(-90);
				Thread.sleep(500);
				if(rc.getSonarDistance()<ySonarDistance){
					ySonarDistance = rc.getSonarDistance();
					pilot.rotate(-90);
					pilot.travel(((ySonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE ) - SECTOR_SIZE / 2, false);
					if(rc.getSonarDistance()<xSonarDistance){
						xSonarDistance = rc.getSonarDistance();
						pilot.rotate(-90);
						pilot.travel(((xSonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
						pilot.rotate(180);
					}
					else{
						pilot.rotate(90);
						pilot.travel(((xSonarDistance+X_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
						}
					}
				else{
					pilot.rotate(90);
					rc.rotateSonarMotorTo(90);
					Thread.sleep(500);
					pilot.travel(((ySonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2,false);
					if(rc.getSonarDistance()<xSonarDistance){
						xSonarDistance = rc.getSonarDistance();
						pilot.rotate(90);
						pilot.travel(((xSonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
						pilot.rotate(180);
						}
					else{
						pilot.rotate(-90);
						pilot.travel(((xSonarDistance+X_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
						}
					}
				
				}
				else{
					pilot.rotate(90);
					pilot.travel(((ySonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
					System.out.println(ySonarDistance);
					Thread.sleep(500);
					if(rc.getSonarDistance()<xSonarDistance){
						xSonarDistance = rc.getSonarDistance();
						pilot.rotate(90);
						pilot.travel(((xSonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
						pilot.rotate(180);
					}
					else{
						pilot.rotate(-90);
						pilot.travel(((xSonarDistance+X_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
					}
				}
			}
		else{
			pilot.travel(((xSonarDistance+X_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
			if(ySonarDistance>40){
				rc.rotateSonarMotorTo(-90);
				Thread.sleep(200);
				if(rc.getSonarDistance()<ySonarDistance){
					ySonarDistance = rc.getSonarDistance();
					pilot.rotate(-90);
					pilot.travel(((ySonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
					pilot.rotate(90);
				}
				else{
					pilot.rotate(90);
					pilot.travel(((ySonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
					pilot.rotate(-90);
					}
			}
			else{
				pilot.rotate(90);
					pilot.travel(((ySonarDistance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
					pilot.rotate(-90);
			}
			
		}
		//System.out.println("ydistance: " + (int) ((ySonarDistance % SECTOR_SIZE) - (SECTOR_SIZE / 2))+ ", xdistance="+ ((xSonarDistance % SECTOR_SIZE) - SECTOR_SIZE / 2));
		rc.rotateSonarMotorTo(0);
	}

//	private void travel(float f, float sonarOffset, boolean b) {
//		DifferentialPilot pilot = rc.getPilot();
//		
//			pilot.travel(f);
//	}

	public void setStraight() throws InterruptedException {
		DifferentialPilot pilot = rc.getPilot();
		rc.rotateSonarMotorTo(0);
		Thread.sleep(200);
		if(rc.getSonarDistance()<SECTOR_SIZE){
			pilot.travel((rc.getSonarDistance()+ X_SONAR_OFFSET % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
		}
		else{
			rc.rotateSonarMotorTo(90);
			Thread.sleep(200);
			if(rc.getSonarDistance()<SECTOR_SIZE){
				double distance = rc.getSonarDistance();
				pilot.rotate(90);
				pilot.travel(((distance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
				pilot.rotate(-90);
			}
			else{
				rc.rotateSonarMotorTo(-90);
				Thread.sleep(200);
				double distance = rc.getSonarDistance();
				pilot.rotate(-90);
				pilot.travel(((distance+Y_SONAR_OFFSET) % SECTOR_SIZE) - SECTOR_SIZE / 2, false);
				pilot.rotate(90);
			}
		}
		rc.rotateSonarMotorTo(0);
		Thread.sleep(200);
	}		
	public boolean findLine() throws InterruptedException{
		DifferentialPilot pilot = rc.getPilot();
		double travelSpeed = pilot.getTravelSpeed();
		pilot.setTravelSpeed(5);
		
		int tries = 0;
		double angle = findValidWay();
		if ((int)angle == 180)
			tries++;
		
		pilot.rotate(angle);
		if (rc.getLightColour() != SensorColour.WHITE)
			pilot.forward();
		
		while(rc.getLightColour() != SensorColour.WHITE) {
			if (rc.getSonarDistance() < SECTOR_SIZE / 2) {
				angle = findValidWay();
				if ((int)angle == 180)
					tries++;
				else
					tries = 0;
				
				if (tries != 2)
					pilot.rotate(angle);
				else {
					if (!orientateToOpening()) {
						rc.immediateStop();
						return false;
					}
					tries = 0;
				}
					
					
				pilot.forward();
			}
			Thread.sleep(1);
		}
		
		rc.immediateStop();
		pilot.setTravelSpeed(travelSpeed);
		return true;
		
	
	}
	private double findValidWay() throws InterruptedException {
		DifferentialPilot pilot = rc.getPilot();
		double travelSpeed = pilot.getTravelSpeed();
		rc.rotateSonarMotorTo(0);
		Thread.sleep(50);
		
		float sonarDistance;
		sonarDistance = rc.getSonarDistance();
		
		if (sonarDistance < SECTOR_SIZE) {		// forward is a wall
			rc.rotateSonarMotorTo(90);
		} else
			return 0.0;
		
		Thread.sleep(50);
		sonarDistance = rc.getSonarDistance();
		
		if (sonarDistance < SECTOR_SIZE) {		// right is a wall
			rc.rotateSonarMotorTo(-90);
		} else {
			rc.rotateSonarMotorTo(0);
			return 90.0;
		}
		
		Thread.sleep(50);
		sonarDistance = rc.getSonarDistance();
		pilot.setTravelSpeed(travelSpeed);
		if (sonarDistance < SECTOR_SIZE) {		// left is a wall
			rc.rotateSonarMotorTo(0);
			return 180.0;
		} else {
			rc.rotateSonarMotorTo(0);
			return -90.0;
		}
	}
	private boolean orientateToOpening() throws InterruptedException {
		float sonarDistance = rc.getSonarDistance();
		DifferentialPilot pilot = rc.getPilot();
		double travelSpeed = pilot.getTravelSpeed();
		
		if (sonarDistance > SECTOR_SIZE) {
			pilot.rotateLeft();
			while (sonarDistance > SECTOR_SIZE) {
				Thread.sleep(5);
				sonarDistance = rc.getSonarDistance();
			}
			rc.immediateStop();
		} else {
			pilot.rotateRight();
			while (sonarDistance < SECTOR_SIZE) {
				if (pilot.getAngleIncrement()>= 360) {
					
					return false;
				}
				Thread.sleep(5);
				sonarDistance =rc.getSonarDistance();
			}
			rc.immediateStop();
		}
		
		pilot.rotateRight();
		Thread.sleep(10);
		sonarDistance = rc.getSonarDistance();
		while(sonarDistance > SECTOR_SIZE) {
			Thread.sleep(5);
			sonarDistance = rc.getSonarDistance();
		}
		rc.immediateStop();
		
		pilot.rotate(- pilot.getAngleIncrement() / 2);
		
		return true;
	}
}
	


